% Code for Fig1
clc
clear all
close all

A = [0.5 -0.3;
     -0.8 0.1];
A5 = [-0.11 0.03;
      0.17 -0.11];
D = [-0.2;0.1];

nx = size(A,1);
nd = size(D,2);

H0 = 1*eye(nx); 
Hw = 0.1*eye(nd);

Phi = zeros(nx,nx,6);
Phi(:,:,1) = eye(nx);
Hk = H0;
Hk1 = zeros(nx,nx);
Hk2 = zeros(nx,nx);
Hk3 = zeros(nx,nx);
Hk4 = zeros(nx,nx);
Hk5 = zeros(nx,nx);

AA = zeros(nx,nx,6);
AA(:,:,1) = A;
AA(:,:,6) = A5;

Nk = 15;
vk = zeros(1,Nk);
Xk = zeros(nx,1)+Hk*unitbox(nx,1);
vk(:,1) = volume(Xk);
HHk = [zeros(1,nx);Hk];
ck = [1;0;0];
XXk = ck+HHk*unitbox(nx,1);        
    
figure(1)
hold on
Options.color='r';
Options.alpha=0.2;
plot(XXk,Options)

for k=2:Nk
    
    Hk_tmp = [AA(:,:,1)*Hk AA(:,:,2)*Hk1 AA(:,:,3)*Hk2 AA(:,:,4)*Hk3 ... 
        AA(:,:,5)*Hk4 AA(:,:,6)*Hk5 D*Hw];
    Hwk6 = Hk5;
    Hk5 = Hk4;
    Hk4 = Hk3;
    Hk3 = Hk2;
    Hk2 = Hk1;
    Hk1 = Hk;
    Hk = Hk_tmp;
    Hk = reduceG(Hk,10);
    
    s = size(Hk,2);
    Xk = zeros(nx,1)+Hk*unitbox(s,1);
    vk(:,k) = volume(Xk);
    ck = [k;0;0];
    HHk = [zeros(1,s);Hk];
    XXk = ck+HHk*unitbox(s,1);        
    
    figure(1)
    hold on
    Options.color='r';
    Options.alpha=0.2;
    plot(XXk,Options)
end

set(gca,'fontsize',12)
xlabel('k','fontSize',12); 
ylabel('x_1','fontSize',12); 
zlabel('x_2','fontSize',12); 
 
figure(2)
hold on
plot(1:Nk,vk,'rx')

% ReachableSetEstimation_method2
% ReachableSetEstimation_method3

